#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
PDO速度控制与里程计计算演示脚本
整合PDO控制、速度反馈和里程计计算功能
"""

import math
import time
import threading
import logging
import sys
import select
import tty
import termios
from datetime import datetime
import os
from typing import Dict, Any
import struct
import socket
import json
from Motor_ctl import (
    init_can_device, 
    close_can_device, 
    Motor_CTL, 
    TYPE_CANFD,
    INVALID_DEVICE_HANDLE
)

from EMM42_CTL import Emm42_CAN_Controller

# 导入达妙电机控制模块与串口
try:
    from DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type
    import serial
    DM_CAN_AVAILABLE = True
    print("✅ DM_CAN电机控制模块导入成功")
except Exception as e:
    DM_CAN_AVAILABLE = False
    print(f"⚠️ DM_CAN电机控制模块导入失败: {e}")


# 导入PH2501测距模块
try:
    sys.path.append(os.path.join(os.path.dirname(__file__), '..', '..', 'Sensor', 'PSH2501'))
    from PH2501 import PSH2501_DToF
    PH2501_AVAILABLE = True
    print("✅ PH2501测距模块导入成功")
except ImportError as e:
    print(f"⚠️ PH2501测距模块导入失败: {e}")
    PH2501_AVAILABLE = False

# 导入STP23L测距模块
try:
    from STP23L import STP23L_RangeFinder
    STP23L_AVAILABLE = True
    print("✅ STP23L测距模块导入成功")
except ImportError as e:
    print(f"⚠️ STP23L测距模块导入失败: {e}")
    STP23L_AVAILABLE = False

# 导入继电器控制模块
try:
    from relay import Relay
    RELAY_AVAILABLE = True
    print("✅ Relay继电器模块导入成功")
except Exception as e:
    RELAY_AVAILABLE = False
    print(f"⚠️ Relay继电器模块导入失败: {e}")

# 小车参数
WHEEL_RADIUS = 0.075  # 轮子半径（米）
WHEEL_BASE = 0.35      # 轮距（米）

# 全局里程计变量
x = 0.0  # X坐标（米）
y = 0.0  # Y坐标（米）
theta = 0.0  # 朝向角度（弧度）
L_speed = 0.0  # 左轮速度（r/min）
R_speed = 0.0  # 右轮速度（r/min）

# 测距相关全局变量
pillar_distance = 0.0  # 立柱电机距离（米）
ranging_active = False  # 测距功能是否激活

# STP23L测距相关全局变量
stp23l_distance = 0.0  # STP23L测距距离（米）
stp23l_ranging_active = False  # STP23L测距功能是否激活

class Robotchassis:
    """底盘电机控制封装"""
    def __init__(self, can_channel_handle, send_id=0x601, response_id=0x581, logger=None):
        self.can = can_channel_handle
        self.logger = logger or logging.getLogger(__name__)
        # 复用原来的 Motor_CTL 初始化方式
        self.motor_ctl = Motor_CTL(self.can, send_id, response_id)

    def setup_velocity_pdo(self):
        """设置速度模式PDO配置（与原控制器实现一致）"""
        self.logger.info("开始配置底盘速度模式PDO...")
        # 1. 设置控制模式
        self.motor_ctl.set_control_mode(self.motor_ctl.SYNC_CONTROL)
        # 2. 初始化电机
        if not self.motor_ctl.initialize_motor():
            self.logger.error("底盘速度模式初始化失败")
            return False
        # 3. 配置RPDO1用于接收目标速度指令（单电机映射，与test_pdo.py一致）
        mapped_objs_rpdo1 = [(self.motor_ctl.OD_TARGET_VELOCITY, 0x03, 4)]
        if not self.motor_ctl.init_pdo('rpdo1', mapped_objs_rpdo1, self.motor_ctl.PDO_TRANSMIT_EVENT):
            self.logger.error("底盘RPDO1初始化失败")
            return False
        # 4. 配置TPDO1用于反馈实际速度（单电机映射，与test_pdo.py一致）
        mapped_objs_tpdo1 = [(self.motor_ctl.OD_VELOCITY_ACTUAL_VALUE, 0x03, 4)]
        if not self.motor_ctl.init_pdo('tpdo1', mapped_objs_tpdo1, self.motor_ctl.PDO_TRANSMIT_EVENT):
            self.logger.error("底盘TPDO1初始化失败")
            return False
        self.logger.info("底盘速度模式PDO配置完成")
        return True

    def send_velocity_command(self, left_speed, right_speed):
        """发送底盘速度指令（带重试机制），与原控制器一致"""
        # 限幅到 [-20, 20]
        left_speed = max(min(left_speed, 20), -20)
        right_speed = max(min(right_speed, 20), -20)
        # 整数化
        left_speed_int = int(left_speed)
        right_speed_int = int(right_speed)
        left_bytes = left_speed_int.to_bytes(2, byteorder='little', signed=True)
        right_bytes = right_speed_int.to_bytes(2, byteorder='little', signed=True)
        pdo_data = list(left_bytes) + list(right_bytes)
        self.logger.info(f"底盘指令 - 左轮: {left_speed:.1f} r/min, 右轮: {right_speed:.1f} r/min")
        # 重试发送
        max_retries = 5
        retry_count = 0
        while retry_count < max_retries:
            if self.motor_ctl.send_pdo('rpdo1', pdo_data):
                time.sleep(0.1)
                self.logger.info("底盘速度指令发送成功")
                return True
            retry_count += 1
            self.logger.warning(f"底盘PDO发送失败，第{retry_count}次重试...")
            time.sleep(0.2)
        self.logger.error("底盘PDO发送失败，达到最大重试次数")
        return False

    def read_real_speed(self, timeout=0.05):
        """读取实际速度反馈（合并原start_velocity_parsing和_velocity_parsing_thread功能）
        
        Args:
            timeout: 接收超时时间（秒）
            
        Returns:
            tuple: (success, left_speed, right_speed) 
                   success: 是否成功读取
                   left_speed: 左轮速度（r/min）
                   right_speed: 右轮速度（r/min）
        """
        try:
            # 读取速度反馈数据
            success, data = self.motor_ctl.receive_standard_frame(0x281, timeout)
            
            if success and data and len(data) >= 4:
                # 正确解析速度数据：4字节数据包含左右轮速度各2字节
                # 数据格式：左轮速度（2字节小端） + 右轮速度（2字节小端）
                left_velocity_raw = int.from_bytes(bytes(data[0:2]), 'little', signed=True)
                right_velocity_raw = int.from_bytes(bytes(data[2:4]), 'little', signed=True)
                
                # 转换为r/min（根据驱动器规格调整系数）
                left_velocity = left_velocity_raw * 0.1
                right_velocity = right_velocity_raw * 0.1
                
                # 记录解析结果到日志
                self.logger.info(f"速度反馈 - 左轮: {left_velocity:.1f} r/min, 右轮: {right_velocity:.1f} r/min | "
                               f"原始数据: {[hex(d) for d in data]}")
                
                return True, left_velocity, right_velocity
            else:
                return False, 0.0, 0.0
                
        except Exception as e:
            self.logger.error(f"读取速度反馈错误: {e}")
            return False, 0.0, 0.0

    def Chassiscontroller(self):
        """底盘控制器示例，提供四个方向的运动控制"""        
        def forward(speed):
            """前进：左轮正转，右轮反转"""
            return self.send_velocity_command(speed, -speed)
        
        def backward(speed):
            """后退：左轮反转，右轮正转"""
            return self.send_velocity_command(-speed, speed)
        
        def right(speed):
            """原地右转：左轮正转，右轮正转"""
            return self.send_velocity_command(speed, speed)
        
        def left(speed):    
            """原地左转：左轮反转，右轮反转"""
            return self.send_velocity_command(-speed, -speed)
        
        # 返回控制函数字典，方便调用
        return {
            'forward': forward,
            'backward': backward,
            'right': right,
            'left': left
        }

class RealTimeDataVisualizer:
    """实时数据可视化类"""
    
    def __init__(self, robotlift=None):
        """初始化UDP数据发送器"""
        self.last_update_time = time.time()
        self.update_interval = 0.1  # 更新间隔（秒）
        self.running = True
        # 立柱电机控制引用，用于读取实时速度
        self.robotlift = robotlift
        
        # 初始化UDP服务器用于数据发布
        self.udp_server_socket = None
        self.udp_port = 8080
        self.udp_enabled = True
        self.connected_clients = set()  # 存储已连接的客户端地址
        self.udp_server_thread = None
        self.udp_server_running = False
        
        try:
            # 创建UDP服务器socket
            self.udp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            # 绑定到所有网络接口的8080端口，允许外部访问
            self.udp_server_socket.bind(('0.0.0.0', self.udp_port))
            self.udp_server_socket.settimeout(1.0)  # 设置超时，便于优雅退出
            
            # 获取本机IP地址
            import socket as sock_module
            hostname = sock_module.gethostname()
            local_ip = sock_module.gethostbyname(hostname)
            
            print(f"✅ UDP服务器初始化成功，监听端口: {self.udp_port}")
            print(f"📡 其他主机可通过以下地址订阅数据:")
            print(f"   - 本机IP: {local_ip}:{self.udp_port}")
            print(f"   - 本机访问: localhost:{self.udp_port}")
            
            # 启动UDP服务器线程
            self._start_udp_server()
            
        except Exception as e:
            print(f"⚠️ UDP服务器初始化失败: {e}")
            self.udp_enabled = False
           
    def display_data(self, left_speed: float, right_speed: float, 
                    x_pos: float, y_pos: float, angle: float, 
                    pillar_dist: float = 0.0, ranging_status: bool = False):
        """发送实时数据（仅通过UDP，不再显示命令行输出）"""
        current_time = time.time()
        if current_time - self.last_update_time < self.update_interval:
            return
        
        self.last_update_time = current_time
        
        # 通过UDP发送实时数据
        self._send_data_via_udp(left_speed, right_speed, x_pos, y_pos, angle, pillar_dist, ranging_status)
    
    def _start_udp_server(self):
        """启动UDP服务器线程"""
        self.udp_server_running = True
        self.udp_server_thread = threading.Thread(target=self._udp_server_thread, daemon=True)
        self.udp_server_thread.start()
    
    def _udp_server_thread(self):
        """UDP服务器线程 - 处理客户端连接请求"""
        print("🔄 UDP服务器线程启动，等待客户端连接...")
        
        while self.udp_server_running:
            try:
                # 接收客户端的连接请求
                data, client_addr = self.udp_server_socket.recvfrom(1024)
                
                # 解析客户端请求
                try:
                    request = json.loads(data.decode('utf-8'))
                    if request.get('action') == 'subscribe':
                        self.connected_clients.add(client_addr)
                        print(f"📱 新客户端连接: {client_addr[0]}:{client_addr[1]}")
                        
                        # 发送确认消息
                        response = {"status": "subscribed", "message": "成功订阅机器人数据"}
                        self.udp_server_socket.sendto(
                            json.dumps(response).encode('utf-8'), client_addr
                        )
                except:
                    # 如果不是JSON格式，可能是简单的连接请求
                    if data.decode('utf-8').strip().lower() in ['connect', 'subscribe', 'hello']:
                        self.connected_clients.add(client_addr)
                        print(f"📱 新客户端连接: {client_addr[0]}:{client_addr[1]}")
                        
            except socket.timeout:
                # 超时是正常的，继续循环
                continue
            except Exception as e:
                if self.udp_server_running:
                    print(f"⚠️ UDP服务器错误: {e}")
                break
        
        print("🔄 UDP服务器线程已停止")
    
    def _send_data_via_udp(self, left_speed: float, right_speed: float, 
                          x_pos: float, y_pos: float, angle: float, 
                          pillar_dist: float = 0.0, ranging_status: bool = False):
        """通过UDP广播实时数据给所有订阅的客户端"""
        if not self.udp_enabled or not self.udp_server_socket:
            return
        
        try:
            # 读取立柱实时速度
            pillar_speed_rpm = None
            pillar_speed_dir = None
            if self.robotlift:
                try:
                    speed_info = self.robotlift.read_real_speed()
                    if isinstance(speed_info, tuple):
                        pillar_speed_rpm, pillar_speed_dir = speed_info
                    elif speed_info is not None:
                        pillar_speed_rpm = speed_info
                except Exception:
                    pass
            # 准备数据
            data = {
                "timestamp": datetime.now().isoformat(),
                "left_speed": left_speed,
                "right_speed": right_speed,
                "x_position": x_pos,
                "y_position": y_pos,
                "angle_rad": angle,
                "angle_deg": math.degrees(angle),
                "pillar_distance": pillar_dist,
                "ranging_status": ranging_status,
                "pillar_speed_rpm": pillar_speed_rpm,
                "pillar_speed_direction": pillar_speed_dir
            }
            
            # 转换为JSON字符串
            json_data = json.dumps(data).encode('utf-8')
            
            # 发送给所有已连接的客户端
            disconnected_clients = set()
            for client_addr in self.connected_clients.copy():
                try:
                    self.udp_server_socket.sendto(json_data, client_addr)
                except Exception:
                    # 客户端可能已断开连接
                    disconnected_clients.add(client_addr)
            
            # 移除断开连接的客户端
            self.connected_clients -= disconnected_clients
            
        except Exception as e:
            # 静默处理UDP发送错误，避免影响主程序运行
            pass
    
    def stop(self):
        """停止可视化和UDP服务器"""
        self.running = False
        
        # 停止UDP服务器
        if self.udp_server_running:
            self.udp_server_running = False
            print("🔄 正在停止UDP服务器...")
            
            # 等待服务器线程结束
            if self.udp_server_thread and self.udp_server_thread.is_alive():
                self.udp_server_thread.join(timeout=2.0)
        
        # 关闭UDP服务器socket
        if self.udp_server_socket:
            try:
                self.udp_server_socket.close()
                print("✅ UDP服务器已关闭")
            except Exception as e:
                print(f"⚠️ 关闭UDP服务器时出错: {e}")
        
        # 清空客户端列表
        self.connected_clients.clear()

class Robotlift:
    """立柱电机封装：基于EMM42驱动，统一速度/停止/解除堵转/测速接口"""

    def __init__(self, emm42_ctl: Emm42_CAN_Controller = None, channel_handle=None, device_id: int = 1):
        self.logger = logging.getLogger(__name__)
        self.speed_limit_rpm = 100
        self.emm42 = None
        try:
            if emm42_ctl is not None:
                self.emm42 = emm42_ctl
            elif channel_handle is not None:
                self.emm42 = Emm42_CAN_Controller(channel_handle=channel_handle, device_id=device_id)
                # 使能电机
                try:
                    self.emm42.motor_enable(enable=True)
                except Exception:
                    pass
        except Exception as e:
            self.logger.warning(f"Robotlift初始化失败: {e}")
            self.emm42 = None

        if self.emm42 is None:
            self.logger.warning("Robotlift未绑定EMM42控制器，功能受限")

    def is_ready(self) -> bool:
        return self.emm42 is not None

    def set_speed(self, direction: int, speed_rpm: int, accel_level: int = 0x00, sync_flag: int = 0x00) -> bool:
        """速度模式控制（限速0~100 rpm）"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法设置立柱速度")
            return False
        try:
            # 限速保护：0~100 rpm
            clamped_speed = max(0, min(int(speed_rpm), self.speed_limit_rpm))
            return self.emm42.speed_control(direction=direction, speed_rpm=clamped_speed,
                                            accel_level=accel_level, sync_flag=sync_flag)
        except Exception as e:
            self.logger.error(f"设置立柱速度失败: {e}")
            return False

    def immediate_stop(self, sync_flag: int = 0x00) -> bool:
        """立即停止"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法立即停止")
            return False
        try:
            return self.emm42.immediate_stop(sync_flag=sync_flag)
        except Exception as e:
            self.logger.error(f"立柱立即停止失败: {e}")
            return False

    def clear_lock_protection(self) -> bool:
        """解除堵转保护"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法解除堵转保护")
            return False
        try:
            return self.emm42.clear_lock_protection()
        except Exception as e:
            self.logger.error(f"解除堵转保护失败: {e}")
            return False

    def read_real_speed(self):
        """读取实时转速与方向"""
        if not self.is_ready():
            return None
        try:
            return self.emm42.read_real_speed()
        except Exception:
            return None

    def Liftcontroller(self):
        def up(speed):
            self.set_speed(direction=0, speed_rpm=speed)
        def down(speed):
            self.set_speed(direction=1, speed_rpm=speed)
        def stop():
            self.immediate_stop()

class Robotarm:
    """机械臂控制类：读取限位配置并通过DM_CAN的MIT模式设定位置"""

    def __init__(self, config_path: str = "/home/tz2/T9-Test/arm_config.json"):
        if not DM_CAN_AVAILABLE:
            raise RuntimeError("DM_CAN模块不可用，无法初始化Robotarm")

        # 读取配置
        self.config_path = config_path
        self.config = self._load_config(self.config_path)
        self.arm_side = self.config.get("arm_side", "left")

        # 串口初始化
        serial_cfg = self.config.get("serial", {})
        port = serial_cfg.get("port", "/dev/ttyACM0")
        baudrate = serial_cfg.get("baudrate", 921600)
        timeout = serial_cfg.get("timeout", 0.5)

        self.serial = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
        self.mc = MotorControl(self.serial)

        # 电机类型字符串到枚举映射
        self._type_map = {
            "DM4310": DM_Motor_Type.DM4310,
            "DM4310_48V": DM_Motor_Type.DM4310_48V,
            "DM4340": DM_Motor_Type.DM4340,
            "DM4340_48V": DM_Motor_Type.DM4340_48V,
            "DM6006": DM_Motor_Type.DM6006,
            "DM8006": DM_Motor_Type.DM8006,
            "DM8009": DM_Motor_Type.DM8009,
            "DM10010L": DM_Motor_Type.DM10010L,
            "DM10010": DM_Motor_Type.DM10010,
            "DMH3510": DM_Motor_Type.DMH3510,
            "DMH6215": DM_Motor_Type.DMH6215,
            "DMG6220": DM_Motor_Type.DMG6220,
        }

        # 注册电机并建立ID映射
        self.motors_by_id = {}
        for m in self.config.get("motors", []):
            m_type_str = m.get("type", "DM4310")
            m_type = self._type_map.get(m_type_str, DM_Motor_Type.DM4310)
            slave_id = int(m.get("slave_id"))
            master_id = int(m.get("master_id", 0))
            motor = Motor(m_type, slave_id, master_id)
            self.mc.addMotor(motor)
            self.motors_by_id[slave_id] = motor

        # 读取位置限位
        self.position_limits_by_id = {}
        self._load_position_limits()

        # 切换到MIT模式（若固件支持）
        for motor in self.motors_by_id.values():
            try:
                self.mc.switchControlMode(motor, Control_Type.MIT)
            except Exception:
                # 保持静默，旧固件可能不支持
                pass

    def _load_config(self, path: str) -> dict:
        with open(path, "r", encoding="utf-8") as f:
            return json.load(f)

    def _parse_id(self, id_str: str) -> int:
        try:
            if isinstance(id_str, int):
                return id_str
            id_str = str(id_str)
            if id_str.startswith("0x") or id_str.startswith("0X"):
                return int(id_str, 16)
            return int(id_str)
        except Exception:
            raise ValueError(f"电机ID解析失败: {id_str}")

    def get_arm_torque(self):
        """获取机械臂所有电机的力矩
        
        Returns:
            dict: 电机ID到力矩值的映射，格式为 {slave_id: torque_value}
        """
        torque_data = {}
        
        # 先接收一次数据以更新电机状态
        try:
            self.mc.recv()
        except Exception as e:
            print(f"⚠️ 接收电机数据时出错: {e}")
        
        # 遍历所有注册的电机，获取力矩数据
        for slave_id, motor in self.motors_by_id.items():
            try:
                torque = motor.getTorque()
                torque_data[slave_id] = torque
            except Exception as e:
                print(f"⚠️ 获取电机{slave_id}力矩时出错: {e}")
                torque_data[slave_id] = 0.0
        
        return torque_data

    def get_arm_temp(self):
        """获取机械臂所有电机的温度
        
        Returns:
            dict: 电机ID到温度值的映射，格式为 {slave_id: {'mos_temp': float, 'rotor_temp': float}}
        """
        temp_data = {}
        
        # 先接收一次数据以更新电机状态
        try:
            self.mc.recv()
        except Exception as e:
            print(f"⚠️ 接收电机数据时出错: {e}")
        
        # 遍历所有注册的电机，获取温度数据
        for slave_id, motor in self.motors_by_id.items():
            try:
                mos_temp, rotor_temp = motor.getTemperature()
                temp_data[slave_id] = {
                    'mos_temp': mos_temp,
                    'rotor_temp': rotor_temp
                }
            except Exception as e:
                print(f"⚠️ 获取电机{slave_id}温度时出错: {e}")
                temp_data[slave_id] = {
                    'mos_temp': 0.0,
                    'rotor_temp': 0.0
                }
        
        return temp_data

    def _load_position_limits(self):
        side_limits = self.config.get("motor_limits", {}).get(self.arm_side, {})
        for id_str, lim in side_limits.items():
            slave_id = self._parse_id(id_str)
            min_val = lim.get("min")
            max_val = lim.get("max")
            self.position_limits_by_id[slave_id] = (min_val, max_val)

    def _clamp_position(self, slave_id: int, pos: float) -> float:
        lim = self.position_limits_by_id.get(slave_id)
        if not lim:
            return pos
        min_val, max_val = lim
        if min_val is not None and pos < min_val:
            return min_val
        if max_val is not None and pos > max_val:
            return max_val
        return pos

    def set_pos(self, slave_id: int, position: float, kp: float = 50.0, kd: float = 1.0,
                dq: float = 0.0, tau: float = 0.0) -> float:
        """设置指定电机位置（MIT模式），自动应用限位

        Args:
            slave_id: 电机从机ID
            position: 目标位置（rad）
            kp: 位置环KP（0~500，DM固件范围）
            kd: 位置环KD（0~5，DM固件范围）
            dq: 期望速度（rad/s）
            tau: 期望力矩

        Returns:
            实际发送的目标位置（经过限位夹紧后）
        """
        if slave_id not in self.motors_by_id:
            raise KeyError(f"未找到电机，SlaveID={slave_id}")

        motor = self.motors_by_id[slave_id]

        # 限位夹紧
        q_target = self._clamp_position(slave_id, position)

        # 发送MIT控制
        self.mc.controlMIT(motor, kp, kd, q_target, dq, tau)
        return q_target

    def Armcontroller(self):
        """机械臂控制器示例"""  
             
        def stop():
            """停止机械臂运动"""
            set_pos(self.arm_motor_id, 0.0, kp=0.0, kd=0.0, dq=0.0, tau=0.0)
            pass
        pass
    
class Tof:
    """ToF测距封装类：解耦PH2501与STP23L的使用，并维护原有全局变量更新"""

    def __init__(self):
        self.logger = logging.getLogger(__name__)

        # PH2501测距模块
        self.psh2501 = None
        self.ranging_active = False
        self.ranging_thread = None
        self.ranging_monitoring = False
        self.ranging_lock = threading.Lock()
        self.pillar_distance = 0.0

        if PH2501_AVAILABLE:
            try:
                self.psh2501 = PSH2501_DToF()
                if self.psh2501.open_serial():
                    print("✅ PH2501测距模块初始化成功")
                    self.logger.info("PH2501测距模块初始化成功")
                else:
                    print("⚠️ PH2501测距模块串口打开失败")
                    self.logger.warning("PH2501测距模块串口打开失败")
                    self.psh2501 = None
            except Exception as e:
                print(f"⚠️ PH2501测距模块初始化失败: {e}")
                self.logger.warning(f"PH2501测距模块初始化失败: {e}")
                self.psh2501 = None
        else:
            print("⚠️ PH2501测距模块不可用")
            self.logger.warning("PH2501测距模块不可用")

        # STP23L测距模块
        self.stp23l = None
        self.stp23l_ranging_active = False
        self.stp23l_ranging_thread = None
        self.stp23l_ranging_monitoring = False
        self.stp23l_ranging_lock = threading.Lock()
        self.stp23l_distance = 0.0

        if STP23L_AVAILABLE:
            try:
                self.stp23l = STP23L_RangeFinder()
                print("✅ STP23L测距模块初始化成功")
                self.logger.info("STP23L测距模块初始化成功")
            except Exception as e:
                print(f"⚠️ STP23L测距模块初始化失败: {e}")
                self.logger.warning(f"STP23L测距模块初始化失败: {e}")
                self.stp23l = None
        else:
            print("⚠️ STP23L测距模块不可用")
            self.logger.warning("STP23L测距模块不可用")

    def _ranging_thread(self):
        """PH2501测距线程 - 持续读取PH2501测距数据"""
        self.logger.info("测距线程启动")

        ranging_interval = 0.1  # 100ms测距间隔

        try:
            while self.ranging_monitoring:
                start_time = time.time()

                # 检查测距是否激活
                with self.ranging_lock:
                    if not self.ranging_active:
                        time.sleep(ranging_interval)
                        continue

                # 检查PH2501模块是否可用
                if not self.psh2501:
                    time.sleep(ranging_interval)
                    continue

                try:
                    # 读取测距数据（56字节）
                    data = self.psh2501.ser.read(56)
                    if len(data) == 56:
                        # 解析并显示距离数据
                        result = self.psh2501.parse_open_flow_reply(data)
                        if result:
                            # 更新全局测距变量
                            global pillar_distance
                            with self.ranging_lock:
                                pillar_distance = result['distance_m']
                                self.pillar_distance = result['distance_m']
                            # 记录到日志（避免过于频繁）
                            if hasattr(self, '_last_ranging_log') and time.time() - self._last_ranging_log > 2.0:
                                self.logger.info(f"测距数据: {result['distance_m']:.4f}m, 信噪比: {result['snr']}")
                                self._last_ranging_log = time.time()
                            elif not hasattr(self, '_last_ranging_log'):
                                self._last_ranging_log = time.time()
                    else:
                        if len(data) > 0:
                            self.logger.warning(f"接收数据长度异常：期望56字节，实际{len(data)}字节")
                except Exception as e:
                    self.logger.error(f"读取测距数据异常：{str(e)}")

                # 控制周期等待
                elapsed_time = time.time() - start_time
                sleep_time = max(0, ranging_interval - elapsed_time)
                time.sleep(sleep_time)

        except Exception as e:
            self.logger.error(f"测距线程错误: {e}")

        self.logger.info("测距线程结束")

    def start_ranging(self):
        """启动PH2501测距功能"""
        if not self.psh2501:
            print("⚠️ PH2501测距模块不可用，无法启动测距")
            return False

        if self.ranging_monitoring:
            self.logger.warning("测距线程已在运行中")
            return True

        # 发送开流指令
        try:
            open_reply = self.psh2501.send_command("open_flow")
            if not open_reply:
                print("❌ 发送开流指令失败")
                return False

            result = self.psh2501.parse_open_flow_reply(open_reply)
            if not result:
                print("❌ 解析开流指令回复失败")
                return False

            print("✅ PH2501测距模块开流成功")
            self.logger.info("PH2501测距模块开流成功")
        except Exception as e:
            print(f"❌ PH2501测距模块开流失败: {e}")
            self.logger.error(f"PH2501测距模块开流失败: {e}")
            return False

        # 启动测距线程
        with self.ranging_lock:
            self.ranging_active = True

        self.ranging_monitoring = True
        self.ranging_thread = threading.Thread(
            target=self._ranging_thread,
            daemon=True
        )
        self.ranging_thread.start()
        self.logger.info("测距线程已启动")

        # 更新全局变量
        global ranging_active
        ranging_active = True

        return True

    def stop_ranging(self):
        """停止PH2501测距功能"""
        if not self.psh2501:
            return False

        # 停止测距线程
        with self.ranging_lock:
            self.ranging_active = False

        self.ranging_monitoring = False
        if self.ranging_thread:
            self.ranging_thread.join(timeout=1)

        # 发送关流指令
        try:
            close_reply = self.psh2501.send_command("close_flow")
            if close_reply:
                result = self.psh2501.parse_close_flow_reply(close_reply)
                if result is True:
                    print("✅ PH2501测距模块关流成功")
                    self.logger.info("PH2501测距模块关流成功")
                else:
                    print("⚠️ PH2501测距模块关流失败")
                    self.logger.warning("PH2501测距模块关流失败")
            else:
                print("⚠️ 发送关流指令失败")
                self.logger.warning("发送关流指令失败")
        except Exception as e:
            print(f"⚠️ PH2501测距模块关流异常: {e}")
            self.logger.error(f"PH2501测距模块关流异常: {e}")

        # 更新全局变量
        global ranging_active
        ranging_active = False

        return True

    def toggle_ranging(self):
        """切换PH2501测距功能开关"""
        global ranging_active
        # 使用Tof自身状态进行判断
        if self.ranging_active:
            # 当前激活，则关闭
            if self.stop_ranging():
                print("🔴 测距功能已关闭")
                return False
            else:
                print("⚠️ 关闭测距功能失败")
                return self.ranging_active
        else:
            # 当前关闭，则打开
            if self.start_ranging():
                print("🟢 测距功能已开启")
                return True
            else:
                print("⚠️ 开启测距功能失败")
                return self.ranging_active

    def _stp23l_ranging_thread(self):
        """STP23L测距线程 - 持续读取STP23L测距数据"""
        self.logger.info("STP23L测距线程启动")

        try:
            while self.stp23l_ranging_monitoring:
                # 检查测距是否激活
                with self.stp23l_ranging_lock:
                    if not self.stp23l_ranging_active:
                        time.sleep(0.1)
                        continue

                # 检查STP23L模块是否可用
                if not self.stp23l:
                    time.sleep(0.1)
                    continue

                try:
                    # 获取STP23L测距数据
                    distance = self.stp23l.get_distance()

                    # 更新全局测距变量
                    global stp23l_distance
                    with self.stp23l_ranging_lock:
                        stp23l_distance = distance
                        self.stp23l_distance = distance

                    # 记录到日志（避免过于频繁）
                    if hasattr(self, '_last_stp23l_ranging_log') and time.time() - self._last_stp23l_ranging_log > 2.0:
                        self.logger.info(f"STP23L测距数据: {distance:.4f}m")
                        self._last_stp23l_ranging_log = time.time()
                    elif not hasattr(self, '_last_stp23l_ranging_log'):
                        self._last_stp23l_ranging_log = time.time()

                except Exception as e:
                    self.logger.error(f"读取STP23L测距数据异常：{str(e)}")

                time.sleep(0.1)  # 控制读取频率

        except Exception as e:
            self.logger.error(f"STP23L测距线程错误: {e}")

        self.logger.info("STP23L测距线程结束")

    def start_stp23l_ranging(self):
        """启动STP23L测距功能"""
        if not self.stp23l:
            print("⚠️ STP23L测距模块不可用，无法启动测距")
            return False

        if self.stp23l_ranging_monitoring:
            self.logger.warning("STP23L测距线程已在运行中")
            return True

        # 启动STP23L测距
        if not self.stp23l.start_ranging():
            print("❌ STP23L测距模块启动失败")
            return False

        print("✅ STP23L测距模块启动成功")
        self.logger.info("STP23L测距模块启动成功")

        # 启动测距线程
        with self.stp23l_ranging_lock:
            self.stp23l_ranging_active = True

        self.stp23l_ranging_monitoring = True
        self.stp23l_ranging_thread = threading.Thread(
            target=self._stp23l_ranging_thread,
            daemon=True
        )
        self.stp23l_ranging_thread.start()
        self.logger.info("STP23L测距线程已启动")

        # 更新全局变量
        global stp23l_ranging_active
        stp23l_ranging_active = True

        return True

    def stop_stp23l_ranging(self):
        """停止STP23L测距功能"""
        if not self.stp23l:
            return False

        # 停止测距线程
        with self.stp23l_ranging_lock:
            self.stp23l_ranging_active = False

        self.stp23l_ranging_monitoring = False
        if self.stp23l_ranging_thread:
            self.stp23l_ranging_thread.join(timeout=1)

        # 停止STP23L测距
        if self.stp23l.stop_ranging():
            print("✅ STP23L测距模块停止成功")
            self.logger.info("STP23L测距模块停止成功")
        else:
            print("⚠️ STP23L测距模块停止失败")
            self.logger.warning("STP23L测距模块停止失败")

        # 更新全局变量
        global stp23l_ranging_active
        stp23l_ranging_active = False

        return True

    def toggle_stp23l_ranging(self):
        """切换STP23L测距功能开关"""
        global stp23l_ranging_active
        # 使用Tof自身状态进行判断
        if self.stp23l_ranging_active:
            # 当前激活，则关闭
            if self.stop_stp23l_ranging():
                print("🔴 STP23L测距功能已关闭")
                return False
            else:
                print("⚠️ 关闭STP23L测距功能失败")
                return self.stp23l_ranging_active
        else:
            # 当前关闭，则打开
            if self.start_stp23l_ranging():
                print("🟢 STP23L测距功能已开启")
                return True
            else:
                print("⚠️ 开启STP23L测距功能失败")
                return self.stp23l_ranging_active

def get_key():
    """跨平台键盘输入函数"""
    if sys.platform == 'win32':
        import msvcrt
        if msvcrt.kbhit():
            return msvcrt.getch().decode('utf-8')
    else:
        # Linux/Unix
        import select
        import tty
        import termios
        
        # 保存终端设置
        old_settings = termios.tcgetattr(sys.stdin)
        try:
            tty.setraw(sys.stdin.fileno())
            if select.select([sys.stdin], [], [], 0.1)[0]:
                char = sys.stdin.read(1)
                return char
        finally:
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
    return None

class PDOOdometryController:
    """PDO控制与里程计计算控制器"""
    
    def __init__(self, send_id=0x601, response_id=0x581, response_timeout=0.5):
        """
        初始化PDO里程计控制器
        
        Args:
            send_id: SDO客户端发送ID（默认0x601）
            response_id: SDO服务器响应ID（默认0x581）
        """
        print("🤖 初始化PDO里程计控制器...")
        
        # 初始化日志系统
        self._setup_logging()
        
        # 初始化CAN设备
        self.m_dev, self.dev_ch0, self.dev_ch1 = init_can_device(
            channels=[0,1],
            baud_rate=500000,
            dbit_baud_rate=500000,
            can_type=TYPE_CANFD
        )
        
        if self.m_dev == INVALID_DEVICE_HANDLE or not self.dev_ch0:
            print("❌ CAN设备初始化失败，请检查硬件连接")
            raise Exception("CAN设备初始化失败")
        
        print(f"✅ CAN设备初始化成功 - 设备句柄: {self.m_dev}, 通道0句柄: {self.dev_ch0}")
        
        # 创建底盘控制封装（Robotchassis）替代直接使用 Motor_CTL
        self.chassis = Robotchassis(self.dev_ch0, send_id, response_id, logger=self.logger)
        
        # 立柱电机封装（Robotlift），用于统一限速和测速监控
        self.robotlift = None
        try:
            if self.dev_ch1:
                # 基于 CAN 通道1 初始化 Robotlift
                self.robotlift = Robotlift(channel_handle=self.dev_ch1, device_id=1)
                if self.robotlift.is_ready():
                    print("✅ 立柱电机封装 Robotlift 初始化完成（限速≤100 rpm）")
                else:
                    print("⚠️ Robotlift 未就绪，立柱速度限速与实时监控不可用")
            else:
                # 通道不可用时，不创建 Robotlift，保持为 None
                self.robotlift = None
                print("⚠️ 无可用的 CAN 通道1，Robotlift 未初始化")
        except Exception as e:
            self.robotlift = None
            print(f"⚠️ Robotlift 初始化失败：{e}")
        
        # 速度反馈解析相关变量
        self.velocity_parsing_monitoring = False
        self.velocity_parsing_thread = None
        self.velocity_parsing_interval = 0.05  # 更短的间隔用于实时解析
        
        # 里程计计算相关变量
        self.odometry_monitoring = False
        self.odometry_thread = None
        self.last_update_time = time.time()
        
        # 线程安全的速度数据共享
        self.left_speed = 0.0
        self.right_speed = 0.0
        self.speed_lock = threading.Lock()
        
        # 角度控制相关变量
        self.angle_control_monitoring = False
        self.angle_control_thread = None
        self.target_angle = 0.0  # 目标角度（弧度）
        self.angle_control_active = False  # 角度控制是否激活
        self.angle_kp = 1.2  # 比例系数（减小以减少超调）
        self.angle_kd = 0.8  # 微分系数（增大以增强阻尼）
        self.last_angle_error = 0.0  # 上一次角度误差
        self.angle_control_lock = threading.Lock()
        
        # 直线运动控制相关变量
        self.position_control_monitoring = False
        self.position_control_thread = None
        self.target_x = 0.0  # 目标X坐标（米）
        self.target_y = 0.0  # 目标Y坐标（米）
        self.position_control_active = False  # 位置控制是否激活
        self.position_kp = 2.0  # 位置比例系数
        self.position_ki = 0.1  # 位置积分系数
        self.position_integral = 0.0  # 位置误差积分
        self.position_control_lock = threading.Lock()
        
        # 实时数据可视化（注入 Robotlift 以监控立柱实时速度与超速告警）
        self.visualizer = RealTimeDataVisualizer(robotlift=self.robotlift)
        self.visualization_monitoring = False
        self.visualization_thread = None
        
        # 测距模块封装（ToF）
        self.tof = Tof()
        print("✅ ToF测距模块封装初始化完成")

        print("✅ PDO里程计控制器初始化完成")


    @property
    def psh2501(self):
        return getattr(self, 'tof', None).psh2501 if hasattr(self, 'tof') else None

    @property
    def stp23l(self):
        return getattr(self, 'tof', None).stp23l if hasattr(self, 'tof') else None

    @property
    def ranging_active(self):
        return getattr(self, 'tof', None).ranging_active if hasattr(self, 'tof') else False

    @property
    def stp23l_ranging_active(self):
        return getattr(self, 'tof', None).stp23l_ranging_active if hasattr(self, 'tof') else False
    
    def _setup_logging(self):
        """设置日志系统"""
        # 创建日志文件名（包含时间戳）
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        log_filename = f"robot_movement_{timestamp}.log"
        
        # 配置日志
        logging.basicConfig(
            level=logging.INFO,
            format='%(asctime)s - %(levelname)s - %(message)s',
            handlers=[
                logging.FileHandler(log_filename, encoding='utf-8'),
                logging.StreamHandler()  # 同时输出到控制台
            ]
        )
        
        self.logger = logging.getLogger(__name__)
        self.logger.info(f"机器人移动日志系统已启动，日志文件: {log_filename}")
        
        # 记录机器人参数
        self.logger.info(f"机器人参数 - 轮子半径: {WHEEL_RADIUS}m, 轮距: {WHEEL_BASE}m")
        
    def start_odometry_calculation(self):
        """启动里程计计算线程"""
        if self.odometry_monitoring:
            self.logger.warning("里程计计算线程已在运行中")
            return
        
        self.odometry_monitoring = True
        self.last_update_time = time.time()
        self.odometry_thread = threading.Thread(
            target=self._odometry_calculation_thread,
            daemon=True
        )
        self.odometry_thread.start()
        self.logger.info("里程计计算线程已启动")
    
    def _odometry_calculation_thread(self):
        """里程计计算线程"""
        self.logger.info("里程计计算线程启动")
        
        try:
            while self.odometry_monitoring:
                current_time = time.time()
                dt = current_time - self.last_update_time
                
                if dt > 0:
                    # 线程安全地获取当前速度
                    with self.speed_lock:
                        left_speed = self.left_speed
                        right_speed = self.right_speed
                    
                    # 调用里程计更新函数
                    x_new, y_new, theta_new, angular_velocity = self.update_odometry(dt)
                    
                    # 记录当前位置、速度和角速度信息到日志
                    # self.logger.info(f"里程计更新 - X: {x_new:.3f}m, Y: {y_new:.3f}m, θ: {math.degrees(theta_new):.1f}° | "
                    #                 f"速度: 左轮={left_speed:.1f}r/min, 右轮={right_speed:.1f}r/min | "
                    #                 f"角速度: {angular_velocity:.3f}rad/s | "
                    #                 f"时间间隔: {dt:.3f}s")
                    
                    self.last_update_time = current_time
                
                time.sleep(0.1)  # 100ms更新一次
                
        except Exception as e:
            self.logger.error(f"里程计计算错误: {e}")
        
        self.logger.info("里程计计算线程结束")
    
    def start_visualization(self):
        """启动实时数据可视化线程"""
        if self.visualization_monitoring:
            self.logger.warning("实时数据可视化线程已在运行中")
            return
        
        self.visualization_monitoring = True
        self.visualization_thread = threading.Thread(
            target=self._visualization_thread,
            daemon=True
        )
        self.visualization_thread.start()
        self.logger.info("实时数据可视化线程已启动")
    
    def _visualization_thread(self):
        """实时数据可视化线程"""
        self.logger.info("实时数据可视化线程启动")

        try:
            while self.visualization_monitoring:
                # 获取当前速度数据
                with self.speed_lock:
                    left_speed = self.left_speed
                    right_speed = self.right_speed
                
                # 获取当前里程计数据
                global x, y, theta
                
                # 获取当前测距数据（改为从Tof封装读取，避免依赖全局变量）
                if hasattr(self, 'tof') and self.tof:
                    current_pillar_distance = getattr(self.tof, 'pillar_distance', 0.0)
                    current_ranging_active = getattr(self.tof, 'ranging_active', False)
                else:
                    current_pillar_distance = 0.0
                    current_ranging_active = False
                
                # 显示实时数据
                self.visualizer.display_data(
                    left_speed, right_speed, x, y, theta, 
                    current_pillar_distance, current_ranging_active
                )
                
                time.sleep(0.1)  # 100ms更新一次
                
        except Exception as e:
            self.logger.error(f"实时数据可视化错误: {e}")
        
        self.logger.info("实时数据可视化线程结束")
    
    def stop_visualization(self):
        """停止实时数据可视化线程"""
        self.visualization_monitoring = False
        if self.visualization_thread:
            self.visualization_thread.join(timeout=2.0)
        self.visualizer.stop()
        self.logger.info("实时数据可视化线程已停止")
    
    def update_odometry(self, dt):
        """
        更新里程计计算
        
        遵循特殊规则：
        - 左轮为负数（后退），右轮为正数（前进）时，x坐标增加
        - 左轮为正数（前进），右轮为负数（后退）时，x坐标减少
        
        实现原理：
        当左右轮速度符号相反时，机器人整体横向移动，x坐标相应变化
        
        Args:
            dt: 时间间隔（秒）
            
        Returns:
            tuple: (x, y, theta, angular_velocity) 当前位置、朝向和角速度
        """
        global x, y, theta
        
        # 线程安全地获取速度数据
        with self.speed_lock:
            left_speed = self.left_speed
            right_speed = self.right_speed
        
        # 计算轮子线速度（m/s）
        # 将r/min转换为m/s：r/min * (2πR/60)
        conversion_factor = 2 * math.pi * WHEEL_RADIUS / 60
        
        # 直接使用原始速度值，不调整符号
        v_left = left_speed * conversion_factor
        v_right = right_speed * conversion_factor
        
        # 计算位移
        delta_left = v_left * dt
        delta_right = v_right * dt
        
        # 根据运动学规律正确处理不同速度模式
        
        # 首先检查速度是否为0，如果速度为0则不进行任何更新
        if abs(left_speed) < 1e-6 and abs(right_speed) < 1e-6:
            # 速度为0时，不更新位置和角度
            return x, y, theta, 0
        
        if left_speed * right_speed < 0:  # 符号相反：直线运动（前进/后退）
            # 直线运动时，使用差速驱动模型
            # 计算角速度（rad/s）
            angular_velocity = (abs(v_right) - abs(v_left)) / WHEEL_BASE
            delta_theta = angular_velocity * dt
            
            # 计算平均位移，确保即使速度符号相反也能正确计算位移
            delta_D = (abs(delta_right) + abs(delta_left)) / 2
            
            # 直线运动时角度变化应该很小，理想情况下应该没有角度变化
            # 但为了处理实际系统中的微小误差，保留很小的角度变化
            theta += delta_theta
            
            # 根据速度方向确定前进方向
            # 使用更智能的判断：基于速度矢量和机器人朝向
            if (left_speed < 0 and right_speed > 0) or (abs(left_speed) > abs(right_speed) and left_speed < 0):  # 主要前进
                # 前进运动：沿当前朝向移动
                x += delta_D * math.cos(theta)
                y += delta_D * math.sin(theta)
            else:  # 主要后退
                # 后退运动：沿当前朝向的反方向移动
                x -= delta_D * math.cos(theta)
                y -= delta_D * math.sin(theta)
            
        else:  # 符号相同：原地转圈
            # 原地转圈时，主要产生角度变化，位移很小
            # 使用标准差速驱动运动学模型计算角速度
            # 角速度 = (右轮速度 - 左轮速度) / 轮距
            # 计算角速度（rad/s）
            angular_velocity = (v_right + v_left) / WHEEL_BASE
            delta_theta = angular_velocity * dt
            
            # 更新角度
            theta += delta_theta
            
            # 原地转圈时位移很小，可以忽略不计
            # 保持x和y坐标基本不变
        
        # 规范化角度到[0, 2π)
        theta = theta % (2 * math.pi)
        
        return x, y, theta, angular_velocity
    
    def set_target_angle(self, target_angle_rad):
        """
        设置目标角度
        
        Args:
            target_angle_rad: 目标角度（弧度）
        """
        with self.angle_control_lock:
            self.target_angle = target_angle_rad
            self.angle_control_active = True

    def demonstrate_keyboard_control(self):
        """
        键盘控制电机演示函数
        
        功能说明：
        - W键长按：前进（left_speed<0, right_speed>0）
        - S键长按：后退（left_speed>0, right_speed<0）
        - A键长按：左转（left_speed>0, right_speed>0）
        - D键长按：右转（left_speed<0, right_speed<0）
        - U键：速度增加1 r/min
        - I键：速度减少1 r/min
        - Q键：退出键盘控制
        
        初始速度：4 r/min
        速度范围：1-20 r/min
        """
        self.logger.info("开始键盘控制演示")
        print("\n🎮 键盘控制模式已启动")
        print("控制说明：")
        print("  W键: 前进 (left_speed<0, right_speed>0)")
        print("  S键: 后退 (left_speed>0, right_speed<0)")
        print("  A键: 左转 (left_speed>0, right_speed>0)")
        print("  D键: 右转 (left_speed<0, right_speed<0)")
        print("  U键: 速度增加1 r/min")
        print("  I键: 速度减少1 r/min")
        print("  H键: 立柱上升 (100rpm)")
        print("  J键: 立柱下降 (100rpm)")
        print("  K键: 立柱立即停止")
        print("  T键: 切换PH2501测距开关")
        print("  Y键: 切换STP23L测距开关")
        print("  O键: 打开继电器")
        print("  P键: 关闭继电器")
        print("  Q键: 退出键盘控制")
        print("  ————————————————————————————————")
        print("  机械臂控制：")
        print("  1-7键: 选择电机ID（与arm_config.json一致）")
        print("  Z/X: 位置微调 -0.05/+0.05 rad")
        print("  C/V: 位置快调 -0.2/+0.2 rad")
        print("  M: 打印当前选中电机限位")
        print("  初始速度: 4 r/min")
        print("\n请按任意键开始控制...")
        
        # 初始化控制参数
        base_speed = 4.0  # 初始速度
        current_speed = base_speed
        
        # 运动状态
        moving_forward = False
        moving_backward = False
        turning_left = False
        turning_right = False
        
        try:
            # 初始化机械臂控制
            robotarm = None
            if DM_CAN_AVAILABLE:
                try:
                    robotarm = Robotarm()
                    print("✅ 机械臂控制已初始化，串口连接成功")
                except Exception as e:
                    print(f"⚠️ 机械臂控制初始化失败：{e}")
                    robotarm = None

            # 初始化继电器控制
            relay = None
            if RELAY_AVAILABLE:
                try:
                    relay = Relay()
                    print("✅ 继电器控制已初始化")
                except Exception as e:
                    print(f"⚠️ 继电器初始化失败：{e}")
                    relay = None
            
            # 重置里程计到原点
            global x, y, theta
            x, y, theta = 0.0, 0.0, 0.0
            
            # 尝试启动速度反馈解析、里程计计算和实时数据可视化，但不强制要求成功
            try:
                # 使用新的read_real_speed函数替代start_velocity_parsing
                success, left_speed, right_speed = self.chassis.read_real_speed()
                if success:
                    print(f"✅ 速度反馈读取成功: 左轮={left_speed:.1f}r/min, 右轮={right_speed:.1f}r/min")
                else:
                    print("⚠️ 速度反馈读取失败，但键盘控制仍可继续")
                
                self.start_odometry_calculation()
                self.start_visualization()
                time.sleep(0.5)  # 等待线程启动
                print("✅ 里程计和实时数据可视化线程已启动")
            except Exception as e:
                print(f"⚠️ 部分线程启动失败，但键盘控制仍可继续: {e}")
                print("⚠️ 部分功能可能无法正常显示，但电机控制功能正常")
            
            print("\n✅ 键盘控制已就绪，开始控制...")
            
            # 按键状态跟踪变量
            last_key = None
            key_pressed_time = 0
            debounce_timer = 0  # 消抖计时器
            debounce_threshold = 0.3  # 消抖阈值：0.3秒
            
            # 等待用户按下第一个按键
            print("\n⌨️  请按下WASD键开始控制，按T键切换测距，或按Q键退出...")
            print("⌨️  机械臂控制也可直接开始：按1-7选择电机，Z/X/C/V调整位置；继电器：O打开，P关闭")
            while True:
                key = get_key()
                if key and key.lower() in ['w', 'a', 's', 'd', 'q', 'h', 'j', 'k', 't', 'y', 'o', 'p',
                                           '1','2','3','4','5','6','7','z','x','c','v','m']:
                    print(f"🎮 检测到按键输入: {key.upper()}")
                    break
                time.sleep(0.1)
            
            print("🎮 开始控制循环...")
            
            # 机械臂当前选择的电机ID（默认 None，需按数字键选择）
            selected_arm_id = None
            # 记录每个电机最近一次发送的目标位置（用于增量微调）
            arm_last_pos_by_id = {}

            while True:
                # 获取当前按键
                key_pressed = get_key()
                
                if key_pressed:
                    key_pressed = key_pressed.lower()
                    last_key = key_pressed
                    key_pressed_time = time.time()
                    debounce_timer = time.time()  # 重置消抖计时器
                    
                    # 处理按键
                    if key_pressed == 'w':
                        if not moving_forward:
                            self.logger.info(f"前进控制 - 速度: {current_speed} r/min")
                            print(f"🚀 前进 - 速度: {current_speed} r/min")
                            moving_forward = True
                        
                        self.chassis.send_velocity_command(current_speed, -current_speed)
                    elif key_pressed == 's':
                        if not moving_backward:
                            self.logger.info(f"后退控制 - 速度: {current_speed} r/min")
                            print(f"🔙 后退 - 速度: {current_speed} r/min")
                            moving_backward = True
                        
                        self.chassis.send_velocity_command(-current_speed, current_speed)
                    elif key_pressed == 'a':
                        if not turning_left:
                            self.logger.info(f"左转控制 - 速度: {current_speed} r/min")
                            print(f"⬅️ 左转 - 速度: {current_speed} r/min")
                            turning_left = True
                        
                        self.chassis.send_velocity_command(-current_speed, -current_speed)
                    elif key_pressed == 'd':
                        if not turning_right:
                            self.logger.info(f"右转控制 - 速度: {current_speed} r/min")
                            print(f"➡️ 右转 - 速度: {current_speed} r/min")
                            turning_right = True
                        
                        self.chassis.send_velocity_command(current_speed, current_speed)
                    elif key_pressed == 'u':
                        # 速度增加
                        if current_speed < 20:
                            current_speed += 1
                            self.logger.info(f"速度增加至: {current_speed} r/min")
                            print(f"📈 速度增加至: {current_speed} r/min")
                        else:
                            print("⚠️ 已达到最大速度 (20 r/min)")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'i':
                        # 速度减少
                        if current_speed > 1:
                            current_speed -= 1
                            self.logger.info(f"速度减少至: {current_speed} r/min")
                            print(f"📉 速度减少至: {current_speed} r/min")
                        else:
                            print("⚠️ 已达到最小速度 (1 r/min)")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'h':
                        # 立柱上升（顺时针，限速100rpm）
                        if self.robotlift and self.robotlift.is_ready():
                            try:
                                if self.robotlift.set_speed(direction=0x00, speed_rpm=100):
                                    self.logger.info("立柱上升 (Robotlift, 100rpm)")
                                    print("🔄 立柱上升 (100rpm)")
                                else:
                                    self.logger.error("立柱上升失败")
                                    print("❌ 立柱上升失败")
                            except Exception as e:
                                self.logger.error(f"立柱控制异常: {e}")
                                print(f"❌ 立柱控制异常: {e}")
                        else:
                            print("❌ 立柱控制器不可用")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'j':
                        # 立柱下降（逆时针，限速100rpm）
                        if self.robotlift and self.robotlift.is_ready():
                            try:
                                if self.robotlift.set_speed(direction=0x01, speed_rpm=100):
                                    self.logger.info("立柱下降 (Robotlift, 100rpm)")
                                    print("🔄 立柱下降 (100rpm)")
                                else:
                                    self.logger.error("立柱下降失败")
                                    print("❌ 立柱下降失败")
                            except Exception as e:
                                self.logger.error(f"立柱控制异常: {e}")
                                print(f"❌ 立柱控制异常: {e}")
                        else:
                            print("❌ 立柱控制器不可用")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'k':
                        # 立柱立即停止
                        if self.robotlift and self.robotlift.is_ready():
                            try:
                                if self.robotlift.immediate_stop():
                                    self.logger.info("立柱立即停止 (Robotlift)")
                                    print("⏹️ 立柱立即停止")
                                else:
                                    self.logger.error("立柱停止失败")
                                    print("❌ 立柱停止失败")
                            except Exception as e:
                                self.logger.error(f"立柱控制异常: {e}")
                                print(f"❌ 立柱控制异常: {e}")
                        else:
                            print("❌ 立柱控制器不可用")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 't':
                        # 切换PH2501测距开关
                        if PH2501_AVAILABLE:
                            try:
                                self.toggle_ranging()
                            except Exception as e:
                                self.logger.error(f"PH2501测距控制异常: {e}")
                                print(f"❌ PH2501测距控制异常: {e}")
                        else:
                            print("❌ PH2501测距模块不可用")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'y':
                        # 切换STP23L测距开关
                        if STP23L_AVAILABLE:
                            try:
                                self.toggle_stp23l_ranging()
                            except Exception as e:
                                self.logger.error(f"STP23L测距控制异常: {e}")
                                print(f"❌ STP23L测距控制异常: {e}")
                        else:
                            print("❌ STP23L测距模块不可用")
                        time.sleep(0.3)  # 防止连续触发
                    elif key_pressed == 'o':
                        # 打开继电器
                        if RELAY_AVAILABLE:
                            try:
                                if relay is None:
                                    relay = Relay()
                                relay.open()
                                print("🔌 继电器已打开 (ON)")
                            except Exception as e:
                                print(f"❌ 继电器打开失败：{e}")
                        else:
                            print("❌ 继电器模块不可用")
                        time.sleep(0.15)
                    elif key_pressed == 'p':
                        # 关闭继电器
                        if RELAY_AVAILABLE:
                            try:
                                if relay is None:
                                    relay = Relay()
                                relay.close()
                                print("🔌 继电器已关闭 (OFF)")
                            except Exception as e:
                                print(f"❌ 继电器关闭失败：{e}")
                        else:
                            print("❌ 继电器模块不可用")
                        time.sleep(0.15)
                    elif key_pressed == 'q':
                        # 退出控制
                        self.logger.info("退出键盘控制")
                        print("\n🛑 退出键盘控制")
                        break
                    # 机械臂：选择电机ID（1-7），仅接收与配置一致的ID
                    elif key_pressed in ['1','2','3','4','5','6','7']:
                        if robotarm:
                            try:
                                selected_arm_id = int(key_pressed)
                                if selected_arm_id not in robotarm.motors_by_id:
                                    print(f"⚠️ ID={selected_arm_id} 未在arm_config.json注册")
                                    selected_arm_id = None
                                else:
                                    print(f"🎯 已选择机械臂电机 ID={selected_arm_id}")
                                    # 初始化该ID的基准位置：优先使用配置中的 motor_positions，其次使用限位中点
                                    if selected_arm_id not in arm_last_pos_by_id:
                                        init_q = 0.0
                                        try:
                                            side = robotarm.arm_side
                                            mp = robotarm.config.get('motor_positions', {}).get(side, {})
                                            # motor_positions 中的键可能是十六进制字符串
                                            for k_str, q_val in mp.items():
                                                try:
                                                    k_id = robotarm._parse_id(k_str)
                                                except Exception:
                                                    continue
                                                if k_id == selected_arm_id:
                                                    init_q = float(q_val)
                                                    break
                                        except Exception:
                                            pass
                                        # 如果配置没有，则使用限位中点/边界作为初值
                                        if init_q == 0.0:
                                            lim = robotarm.position_limits_by_id.get(selected_arm_id)
                                            if lim:
                                                mn, mx = lim
                                                if mn is not None and mx is not None:
                                                    init_q = (mn + mx) / 2.0
                                                elif mn is not None:
                                                    init_q = mn
                                                elif mx is not None:
                                                    init_q = mx
                                        arm_last_pos_by_id[selected_arm_id] = init_q
                            except Exception as e:
                                print(f"⚠️ 选择电机失败：{e}")
                        else:
                            print("❌ 机械臂控制不可用（DM_CAN未加载）")
                        time.sleep(0.15)
                    # 机械臂：位置微调/快调
                    elif key_pressed in ['z','x','c','v','m']:
                        if not robotarm:
                            print("❌ 机械臂控制不可用（DM_CAN未加载）")
                            time.sleep(0.15)
                            continue
                        if selected_arm_id is None:
                            print("⚠️ 请先按数字键选择一个机械臂电机ID")
                            time.sleep(0.15)
                            continue

                        # 读取当前目标作为基准，如果没有历史则先初始化
                        if selected_arm_id not in arm_last_pos_by_id:
                            # 回退初始化逻辑
                            init_q = 0.0
                            lim = robotarm.position_limits_by_id.get(selected_arm_id)
                            if lim:
                                mn, mx = lim
                                if mn is not None and mx is not None:
                                    init_q = (mn + mx) / 2.0
                                elif mn is not None:
                                    init_q = mn
                                elif mx is not None:
                                    init_q = mx
                            arm_last_pos_by_id[selected_arm_id] = init_q
                        base_q = arm_last_pos_by_id[selected_arm_id]

                        delta = 0.0
                        if key_pressed == 'z':
                            delta = -0.05
                        elif key_pressed == 'x':
                            delta = 0.05
                        elif key_pressed == 'c':
                            delta = -0.2
                        elif key_pressed == 'v':
                            delta = 0.2
                        elif key_pressed == 'm':
                            # 打印限位
                            lim = robotarm.position_limits_by_id.get(selected_arm_id)
                            if lim:
                                print(f"📏 电机{selected_arm_id}限位: min={lim[0]}, max={lim[1]}")
                            else:
                                print(f"📏 电机{selected_arm_id}无配置限位")
                            time.sleep(0.15)
                            continue

                        try:
                            q_target = base_q + delta
                            q_send = robotarm.set_pos(selected_arm_id, q_target, kp=50.0, kd=1.0)
                            arm_last_pos_by_id[selected_arm_id] = q_send
                            print(f"🎯 电机{selected_arm_id}位置设定: 目标={q_target:.3f}rad, 发送={q_send:.3f}rad")
                        except Exception as e:
                            print(f"❌ 机械臂位置设定失败：{e}")
                        time.sleep(0.1)
                
                # 软件消抖机制：只有当按键松开超过消抖阈值时才真正停止电机
                current_time = time.time()
                if last_key and current_time - key_pressed_time > 0.1:
                    # 如果消抖计时器未设置，则开始计时
                    if debounce_timer == 0:
                        debounce_timer = current_time
                    
                    # 只有当按键松开时间超过消抖阈值时才执行停止操作
                    if current_time - debounce_timer > debounce_threshold:
                        if moving_forward and last_key == 'w':
                            self.chassis.send_velocity_command(0, 0)
                            self.logger.info("前进按键松开，停止电机")
                            print("⏹️ 前进按键松开，停止")
                            moving_forward = False
                            last_key = None
                            debounce_timer = 0  # 重置消抖计时器
                        elif moving_backward and last_key == 's':
                            self.chassis.send_velocity_command(0, 0)
                            self.logger.info("后退按键松开，停止电机")
                            print("⏹️ 后退按键松开，停止")
                            moving_backward = False
                            last_key = None
                            debounce_timer = 0  # 重置消抖计时器
                        elif turning_left and last_key == 'a':
                            self.chassis.send_velocity_command(0, 0)
                            self.logger.info("左转按键松开，停止电机")
                            print("⏹️ 左转按键松开，停止")
                            turning_left = False
                            last_key = None
                            debounce_timer = 0  # 重置消抖计时器
                        elif turning_right and last_key == 'd':
                            self.chassis.send_velocity_command(0, 0)
                            self.logger.info("右转按键松开，停止电机")
                            print("⏹️ 右转按键松开，停止")
                            turning_right = False
                            last_key = None
                            debounce_timer = 0  # 重置消抖计时器
                else:
                    # 如果有新按键输入，重置消抖计时器
                    debounce_timer = 0
                
                # 打印当前里程计信息
                self._print_odometry_info()
                
                # 控制循环频率
                time.sleep(0.05)  # 20Hz控制频率
                
        except ImportError as e:
            self.logger.error(f"导入错误: {e}")
            print(f"❌ 导入错误: {e}")
            print("请检查相关模块是否已正确安装")
        except KeyboardInterrupt:
            self.logger.info("键盘控制被中断")
            print("\n🛑 键盘控制被中断")
        except Exception as e:
            self.logger.error(f"键盘控制错误: {e}")
            print(f"❌ 键盘控制错误: {e}")
        finally:
            # 确保电机停止
            self.chassis.send_velocity_command(0, 0)
            # 关闭机械臂串口
            try:
                if 'robotarm' in locals() and robotarm and robotarm.serial:
                    robotarm.serial.close()
                    print("✅ 已关闭机械臂串口")
            except Exception:
                pass
            
            # 停止PH2501测距功能
            if PH2501_AVAILABLE and hasattr(self, 'ranging_active'):
                try:
                    self.stop_ranging()
                except Exception as e:
                    self.logger.error(f"停止PH2501测距功能时出错: {e}")
            
            # 停止STP23L测距功能
            if STP23L_AVAILABLE and hasattr(self, 'stp23l_ranging_active'):
                try:
                    self.stop_stp23l_ranging()
                except Exception as e:
                    self.logger.error(f"停止STP23L测距功能时出错: {e}")
            
            # 停止线程
            self.velocity_parsing_monitoring = False
            self.odometry_monitoring = False
            
            # 等待线程结束
            if self.velocity_parsing_thread:
                self.velocity_parsing_thread.join(timeout=1)
            if self.odometry_thread:
                self.odometry_thread.join(timeout=1)
            
            # 打印最终位置
            self._print_final_position()

            self.logger.info("键盘控制演示结束")
            print("✅ 键盘控制演示结束")
            # 关闭继电器串口
            try:
                if 'relay' in locals() and relay:
                    relay.close_port()
                    print("✅ 已关闭继电器串口")
            except Exception:
                pass
    
    def _print_odometry_info(self):
        """打印当前里程计信息"""
        global x, y, theta
        
        # 安全获取当前速度，避免线程问题
        try:
            with self.speed_lock:
                left_speed = self.left_speed
                right_speed = self.right_speed
            
            # 打印到控制台和日志
            odometry_info = (f"里程计 - X: {x:.3f}m, Y: {y:.3f}m, θ: {math.degrees(theta):.1f}° | "
                            f"速度: 左轮={left_speed:.1f}r/min, 右轮={right_speed:.1f}r/min")
        except Exception:
            # 如果速度获取失败，只显示位置信息
            odometry_info = (f"里程计 - X: {x:.3f}m, Y: {y:.3f}m, θ: {math.degrees(theta):.1f}° | "
                            f"速度: 无法获取")
        
        # 打印到控制台（使用\r实现原地刷新）
        print(f"\r{odometry_info}", end="", flush=True)
        
        # 记录到日志（避免过于频繁）
        if hasattr(self, '_last_odometry_log') and time.time() - self._last_odometry_log > 1.0:
            self.logger.info(odometry_info)
            self._last_odometry_log = time.time()
        elif not hasattr(self, '_last_odometry_log'):
            self._last_odometry_log = time.time()
    
    def _print_final_position(self):
        """打印最终位置信息"""
        global x, y, theta
        
        final_info = (f"最终位置 - X: {x:.3f}m, Y: {y:.3f}m, θ: {math.degrees(theta):.1f}° | "
                     f"总移动距离: {math.sqrt(x**2 + y**2):.3f}m")
        
        print(f"\n🎯 {final_info}")
        self.logger.info(final_info)
        self.last_angle_error = 0.0
    
    def set_pd_parameters(self, kp, kd):
        """
        设置PD控制器参数
        
        Args:
            kp: 比例系数
            kd: 微分系数
        """
        with self.angle_control_lock:
            self.angle_kp = kp
            self.angle_kd = kd
        
        self.logger.info(f"设置PD参数 - Kp: {kp}, Kd: {kd}")
    
    def stop_angle_control(self):
        """停止角度控制"""
        with self.angle_control_lock:
            self.angle_control_active = False
        
        # 停止电机
        self.chassis.send_velocity_command(0, 0)
        self.logger.info("角度控制已停止")
    
    def _angle_control_thread(self):
        """角度控制线程 - 使用PD控制实现角度跟踪"""
        self.logger.info("角度控制线程启动")
        
        control_interval = 0.05  # 50ms控制周期
        
        try:
            while self.angle_control_monitoring:
                start_time = time.time()
                
                # 检查角度控制是否激活
                with self.angle_control_lock:
                    if not self.angle_control_active:
                        time.sleep(control_interval)
                        continue
                    
                    target_angle = self.target_angle
                    kp = self.angle_kp
                    kd = self.angle_kd
                
                # 获取当前角度（线程安全，使用里程计线程更新的角度）
                current_angle = theta  # 使用全局变量theta，由里程计线程更新
                
                # 计算角度误差（考虑角度周期性，适应0-360度范围）
                angle_error = target_angle - current_angle
                
                # 规范化角度误差到[-π, π]范围（使用更精确的角度规范化方法）
                angle_error = (angle_error + math.pi) % (2 * math.pi) - math.pi
                
                # 计算角度误差的微分
                with self.angle_control_lock:
                    angle_error_derivative = (angle_error - self.last_angle_error) / control_interval
                    self.last_angle_error = angle_error
                
                # 自适应PD控制：根据误差大小调整控制强度
                # 当误差较大时使用较大增益，接近目标时减小增益
                abs_error = abs(angle_error)
                
                # 误差分段控制
                if abs_error > 1.0:  # 误差大于57度
                    adaptive_kp = kp * 1.2  # 增大比例增益
                    adaptive_kd = kd * 0.8   # 减小微分增益
                elif abs_error > 0.3:  # 误差大于17度
                    adaptive_kp = kp * 1.0  # 标准增益
                    adaptive_kd = kd * 1.0
                elif abs_error > 0.1:  # 误差大于5.7度
                    adaptive_kp = kp * 0.6  # 减小比例增益
                    adaptive_kd = kd * 1.2   # 增大微分增益
                else:  # 误差小于5.7度
                    adaptive_kp = kp * 0.3  # 大幅减小比例增益
                    adaptive_kd = kd * 1.5   # 大幅增大微分增益
                
                # PD控制计算
                control_output = adaptive_kp * angle_error + adaptive_kd * angle_error_derivative
                
                # 限制控制输出范围
                max_control_output = 8.0  # 最大控制输出（r/min）
                min_control_output = 3.0    # 最小控制输出（r/min）
                
                # 限制控制输出在最小和最大范围内
                if abs(control_output) > 0:
                    # 先确保控制输出不低于最小限幅
                    if 0 < abs(control_output) < min_control_output:
                        control_output = math.copysign(min_control_output, control_output)
                    # 再限制控制输出不超过最大限幅
                    control_output = max(min(control_output, max_control_output), -max_control_output)
                
                # 根据控制输出计算左右轮速度
                # 正值控制输出表示需要逆时针旋转（左轮正转，右轮反转）
                # 负值控制输出表示需要顺时针旋转（左轮反转，右轮正转）
                left_speed = control_output
                right_speed = control_output
                
                # 如果误差很小，停止电机
                if abs_error < 0.05:  # 约2.9度误差范围内认为到达目标
                    left_speed = 0
                    right_speed = 0
                    self.logger.info(f"✅ 角度控制完成 - 当前角度: {math.degrees(current_angle):.1f}°, 目标角度: {math.degrees(target_angle):.1f}°")
                    # 停止角度控制
                    with self.angle_control_lock:
                        self.angle_control_active = False
                
                # 发送速度指令
                self.chassis.send_velocity_command(left_speed, right_speed)
                
                # 记录控制状态
                self.logger.info(f"角度控制 - 误差: {math.degrees(abs_error):.1f}°, 控制输出: {control_output:.2f} r/min")
                
                # 控制周期等待
                elapsed_time = time.time() - start_time
                sleep_time = max(0, control_interval - elapsed_time)
                time.sleep(sleep_time)
                
        except Exception as e:
            self.logger.error(f"角度控制线程错误: {e}")
        
        self.logger.info("角度控制线程结束")
    
    def start_angle_control(self):
        """启动角度控制线程"""
        if self.angle_control_monitoring:
            self.logger.warning("角度控制线程已在运行中")
            return
        
        self.angle_control_monitoring = True
        self.angle_control_thread = threading.Thread(
            target=self._angle_control_thread,
            daemon=True
        )
        self.angle_control_thread.start()
        self.logger.info("角度控制线程已启动")
    
    def _ranging_thread(self):
        """兼容旧接口：委托至Tof内部线程实现"""
        return self.tof._ranging_thread()
    
    def start_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.start_ranging()
    
    def stop_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.stop_ranging()
    
    def toggle_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.toggle_ranging()
    
    def _stp23l_ranging_thread(self):
        """兼容旧接口：委托至Tof内部线程实现"""
        return self.tof._stp23l_ranging_thread()
    
    def start_stp23l_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.start_stp23l_ranging()
    
    def stop_stp23l_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.stop_stp23l_ranging()
    
    def toggle_stp23l_ranging(self):
        """兼容旧接口：委托至Tof"""
        return self.tof.toggle_stp23l_ranging()
    
    def set_target_position(self, target_x_meters):
        """
        设置目标位置（X轴方向）
        
        Args:
            target_x_meters: 目标X轴位置（米）
        """
        with self.position_control_lock:
            self.target_x = target_x_meters
            self.position_control_active = True
            self.position_integral = 0.0  # 重置积分项
        
        self.logger.info(f"设置目标位置: {target_x_meters:.3f}米")
    
    def set_target_position_2d(self, target_x, target_y):
        """
        设置2D目标位置
        
        Args:
            target_x: 目标X轴位置（米）
            target_y: 目标Y轴位置（米）
        """
        with self.position_control_lock:
            self.target_x = target_x
            self.target_y = target_y
            self.position_control_active = True
            self.position_integral = 0.0  # 重置积分项
        
        self.logger.info(f"设置2D目标位置: ({target_x:.3f}, {target_y:.3f})米")
    
    def set_pi_parameters(self, kp=5.0, ki=0.5):
        """
        设置PI控制器参数
        
        Args:
            kp: 比例系数（默认5.0，提高响应速度）
            ki: 积分系数（默认0.5，提高稳态精度）
        """
        with self.position_control_lock:
            self.position_kp = kp
            self.position_ki = ki
        
        self.logger.info(f"设置PI参数 - Kp: {kp}, Ki: {ki}")
    
    def stop_position_control(self):
        """停止位置控制"""
        with self.position_control_lock:
            self.position_control_active = False
        
        # 停止电机
        self.chassis.send_velocity_command(0, 0)
        self.logger.info("位置控制已停止")
    
    def _position_control_thread(self):
        """位置控制线程 - 使用PI控制实现2D位置控制"""
        self.logger.info("位置控制线程启动")
        
        control_interval = 0.02  # 20ms控制周期，提高控制频率
        
        try:
            while self.position_control_monitoring:
                start_time = time.time()
                
                # 检查位置控制是否激活
                with self.position_control_lock:
                    if not self.position_control_active:
                        time.sleep(control_interval)
                        continue
                    
                    target_x = self.target_x
                    target_y = self.target_y
                    kp = self.position_kp
                    ki = self.position_ki
                
                # 获取当前位置和朝向（使用全局变量）
                current_x = x
                current_y = y
                current_angle = theta
                
                # 计算到目标点的直线距离和方向
                dx = target_x - current_x
                dy = target_y - current_y
                distance_to_target = math.sqrt(dx*dx + dy*dy)
                
                # 如果距离很小，认为到达目标
                if distance_to_target < 0.01:  # 1cm误差范围内认为到达目标
                    self.chassis.send_velocity_command(0, 0)
                    self.logger.info(f"✅ 2D位置控制完成 - 当前位置: ({current_x:.3f}, {current_y:.3f})米, 目标位置: ({target_x:.3f}, {target_y:.3f})米")
                    # 停止位置控制
                    with self.position_control_lock:
                        self.position_control_active = False
                    time.sleep(control_interval)
                    continue
                
                # 计算目标方向角度
                target_angle = math.atan2(dy, dx)
                
                # 计算角度误差（适应0-360度角度范围）
                # 将目标角度转换为0-360度范围以匹配current_angle的范围
                target_angle_360 = target_angle % (2 * math.pi)
                if target_angle_360 < 0:
                    target_angle_360 += 2 * math.pi
                
                # 计算角度误差（考虑角度周期性）- 使用更智能的方法选择最短路径
                # 直接计算两个角度之间的差值
                angle_error = target_angle_360 - current_angle
                
                # 智能选择最短路径：如果差值大于π，则选择相反方向（更短路径）
                if angle_error > math.pi:
                    angle_error -= 2 * math.pi
                elif angle_error < -math.pi:
                    angle_error += 2 * math.pi
                
                # 进一步优化：如果差值接近±π，检查是否可以通过±2π调整得到更小误差
                if abs(angle_error) > math.pi:
                    # 尝试向相反方向调整
                    if angle_error > 0:
                        alternative_error = angle_error - 2 * math.pi
                    else:
                        alternative_error = angle_error + 2 * math.pi
                    
                    # 选择绝对值更小的误差
                    if abs(alternative_error) < abs(angle_error):
                        angle_error = alternative_error
                
                # 调试信息：打印角度计算详情
                self.logger.debug(f"角度计算详情 - 目标角度: {math.degrees(target_angle):.1f}°, 目标角度360: {math.degrees(target_angle_360):.1f}°, 当前角度: {math.degrees(current_angle):.1f}°, 原始误差: {math.degrees(target_angle_360 - current_angle):.1f}°, 规范化误差: {math.degrees(angle_error):.1f}°")
                
                # 如果角度误差较大，先调整朝向
                if abs(angle_error) > 0.5:  # 约28.6度误差，降低阈值提高响应速度
                    # 角度控制：使用PD控制调整朝向
                    angle_kp = 2.0  # 提高角度控制比例系数，增强响应
                    
                    # 计算角度控制输出
                    angle_control_output = angle_kp * angle_error
                    
                    # 限制角度控制输出范围
                    max_angle_output = 10.0  # 提高最大角度控制输出
                    min_angle_output = 3.0   # 提高最小角度控制输出
                    if abs(angle_control_output) > 0:
                        # 先确保控制输出不低于最小限幅
                        if 0 < abs(angle_control_output) < min_angle_output:
                            angle_control_output = math.copysign(min_angle_output, angle_control_output)
                        # 再限制控制输出不超过最大限幅
                        angle_control_output = max(min(angle_control_output, max_angle_output), -max_angle_output)
                    
                    
                    left_speed = -angle_control_output
                    right_speed = -angle_control_output
                    
                    self.logger.info(f"角度调整 - 误差: {math.degrees(angle_error):.1f}°, 控制输出: {angle_control_output:.2f} r/min")
                else:
                    # 角度误差很小，进行直线运动控制
                    # 计算直线距离误差
                    position_error = distance_to_target
                    
                    # 更新积分项（带积分限幅防止积分饱和）
                    max_integral = 3.0  # 进一步减小最大积分限幅，防止积分饱和
                    with self.position_control_lock:
                        self.position_integral += position_error * control_interval
                        # 积分限幅
                        self.position_integral = max(min(self.position_integral, max_integral), -max_integral)
                    
                    # PI控制计算
                    control_output = kp * position_error + ki * self.position_integral
                    
                    # 限制控制输出范围
                    max_control_output = 8.0  # 降低最大控制输出，提高控制精度
                    min_control_output = 5.0   # 降低最小控制输出，提高小误差时的控制精度
                    
                    # 限制控制输出在最小和最大范围内
                    if abs(control_output) > 0:
                        # 先确保控制输出不低于最小限幅
                        if 0 < abs(control_output) < min_control_output:
                            control_output = math.copysign(min_control_output, control_output)
                        # 再限制控制输出不超过最大限幅
                        control_output = max(min(control_output, max_control_output), -max_control_output)
                    
                    # 直线运动：左右轮速度相同
                    left_speed = -control_output
                    right_speed = control_output
                    
                    self.logger.info(f"直线运动 - 距离误差: {position_error:.3f}米, 控制输出: {control_output:.2f} r/min")
                
                # 发送速度指令
                self.chassis.send_velocity_command(left_speed, right_speed)
                
                # 控制周期等待
                elapsed_time = time.time() - start_time
                sleep_time = max(0, control_interval - elapsed_time)
                time.sleep(sleep_time)
                
        except Exception as e:
            self.logger.error(f"位置控制线程错误: {e}")
        
        self.logger.info("位置控制线程结束")
    
    def start_position_control(self):
        """启动位置控制线程"""
        if self.position_control_monitoring:
            self.logger.warning("位置控制线程已在运行中")
            return
        
        self.position_control_monitoring = True
        self.position_control_thread = threading.Thread(
            target=self._position_control_thread,
            daemon=True
        )
        self.position_control_thread.start()
        self.logger.info("位置控制线程已启动")

    def stop_all_monitoring(self):
        """停止所有监控线程"""
        self.logger.info("停止所有监控线程...")
        
        self.velocity_parsing_monitoring = False
        self.odometry_monitoring = False
        self.angle_control_monitoring = False
        self.position_control_monitoring = False
        self.visualization_monitoring = False
        
        if self.velocity_parsing_thread:
            self.velocity_parsing_thread.join(timeout=1.0)
        if self.odometry_thread:
            self.odometry_thread.join(timeout=1.0)
        if self.angle_control_thread:
            self.angle_control_thread.join(timeout=1.0)
        if self.position_control_thread:
            self.position_control_thread.join(timeout=1.0)
        if self.visualization_thread:
            self.visualization_thread.join(timeout=1.0)
        
        self.logger.info("所有监控线程已停止")
    
    def cleanup(self):
        """清理资源"""
        self.logger.info("开始清理资源...")
        
        # 停止所有监控
        self.stop_all_monitoring()
        
        # 停止电机
        self.logger.info("停止电机...")
        self.chassis.send_velocity_command(0, 0)
        time.sleep(0.5)
        
        # 关闭CAN设备
        close_can_device(self.m_dev, self.dev_ch0)
        self.logger.info("资源清理完成")
        
        # 记录演示结束
        self.logger.info("机器人移动演示结束")

if __name__ == "__main__":
    #
        # 键盘控制演示
        controller = PDOOdometryController()
        try:
            # 设置PDO配置
            if not controller.chassis.setup_velocity_pdo():
                print("❌ PDO配置失败")
                exit(1)
            
            # 跳过速度反馈检查，直接进入键盘控制
            print("⚠️ 跳过速度反馈检查，直接进入键盘控制模式")
            
            # 启动键盘控制
            controller.demonstrate_keyboard_control()
            
        except KeyboardInterrupt:
            print("\n⏹️ 用户中断演示")
        except Exception as e:
            print(f"\n❌ 演示过程中出现错误: {e}")
        finally:
            # 清理资源
            controller.cleanup()